#include <iostream>
#include <string>

#include "machine_factory.h"
#include "predictors/predictor_target.h"
#include "predictors/predictor_const_v.h"
#include "predictors/predictor_acceleration.h"
#include "predictors/predictor_force.h"
#include "machine.h"
#include "arenas/arena_1dfood.h"
#include "arenas/arena_2dfood.h"
#include "arenas/arena_jumping_ball.h"
#include "arenas/arena_pole_cart.h"
#include "predictors/predictor_link.h"
#include "model.h"
#include "ioconnections.h"
#include "output_const.h"
#include "output_cart_ball.h"

using namespace std;

Machine *Machine_factory::create(std::string id)
{
  if (id == "eater1D")
    return create_eater1D();
  if (id == "eater2D")
    return create_eater2D();
  if (id == "jumping ball")
    return create_jumping_ball();
  if (id == "pole cart")
    return create_pole_cart();
  cout << "ERROR: Machine factory cannot create machine of type: " << id << endl;
  return NULL;
}

Machine *Machine_factory::create_eater1D(void)
{
  Machine *m = new Machine();

  // Create arena
  Arena_1dfood *arena = new Arena_1dfood();
  m->set_arena(arena);

  cout << " Creating machine: Eater 1D" << endl;

  // add predictors
  Predictor_target *p_eat = new Predictor_target("Eat");
  Predictor_const_v *p_walk = new Predictor_const_v("Walk");
  Predictor_const_v *p_start = new Predictor_const_v("Start");

  p_eat->set_parameter(0.0); // target position to eat in
  p_walk->set_parameter(-1.0); // target velocity
  p_start->set_parameter(0.0); // target velocity

  Model *model0 = new Model(p_eat);
  Model *model1 = new Model(p_walk);
  Model *model2 = new Model(p_start);


  model1->add_inhibitor_link(model0); // Don't walk when eating
  model2->add_inhibitor_link(model0); // Don't start when eating

  m->add_model(model0);
  m->add_model(model1);
  m->add_model(model2);

  Output_const *output = new Output_const(1.0);

  m->create_output_connection(model0, arena->eat_function(), "eat", output);
  m->create_output_connection(model1, arena->set_speed_function(), "speed", output);
  m->create_output_connection(model2, arena->set_speed_function(), "speed", output);

  m->create_input_connection(p_eat, 1, arena->get_distance_to_food_p());
  m->create_input_connection(p_walk, 1, arena->get_distance_to_food_p());
  m->create_input_connection(p_start, 1, arena->get_distance_to_food_p());

  m->init();

  return m;
}

Machine *Machine_factory::create_eater2D(void)
{
  Machine *m = new Machine();

  // Create arena
  Arena_2dfood *arena = new Arena_2dfood();
  m->set_arena(arena);

  cout << " Creating machine: Eater 2D" << endl;

  // add predictors
  Predictor_target *p_eat = new Predictor_target("Eat");
  Predictor_target *p_stop_x = new Predictor_target("Stop X");
  Predictor_target *p_stop_y = new Predictor_target("Stop Y");
  Predictor_const_v *p_speed_x = new Predictor_const_v("Speed X");
  Predictor_const_v *p_speed_y = new Predictor_const_v("Speed Y");
  Predictor_const_v *p_start_x = new Predictor_const_v("Start X");
  Predictor_const_v *p_start_y = new Predictor_const_v("Start Y");

  p_eat->set_parameter(0.0); // eat at 0.0 distance
  p_stop_x->set_parameter(0.0); // stop at 0.0 distance
  p_stop_y->set_parameter(0.0); // stop at 0.0 distance
  p_start_x->set_parameter(0.0); // start when velocity is 0.0
  p_start_y->set_parameter(0.0); // start when velocity is 0.0
  p_speed_x->set_parameter(1.0); // target velocity
  p_speed_y->set_parameter(1.0); // target velocity

  Model *model_eat = new Model(p_eat);
  Model *model_stop_x = new Model(p_stop_x);
  Model *model_stop_y = new Model(p_stop_y);
  Model *model_speed_x = new Model(p_speed_x);
  Model *model_speed_y = new Model(p_speed_y);
  Model *model_start_x = new Model(p_start_x);
  Model *model_start_y = new Model(p_start_y);

  model_speed_x->add_inhibitor_link(model_stop_x);
  model_speed_y->add_inhibitor_link(model_stop_y);
  model_start_x->add_inhibitor_link(model_speed_x);
  model_start_y->add_inhibitor_link(model_speed_y);
  model_start_x->add_inhibitor_link(model_stop_x);
  model_start_y->add_inhibitor_link(model_stop_y);

  m->add_model(model_eat);
  m->add_model(model_stop_x);
  m->add_model(model_stop_y);
  m->add_model(model_speed_x);
  m->add_model(model_speed_y);
  m->add_model(model_start_x);
  m->add_model(model_start_y);

  Output_const *output_1 = new Output_const(1.0);

  m->create_output_connection(model_eat, arena->eat_function(), "eat", output_1);
  m->create_output_connection(model_speed_x, arena->set_speed_x_function(), "x speed", output_1);
  m->create_output_connection(model_speed_y, arena->set_speed_y_function(), "y speed", output_1);
  m->create_output_connection(model_start_x, arena->set_speed_x_function(), "x speed", output_1);
  m->create_output_connection(model_start_y, arena->set_speed_y_function(), "y speed", output_1);

  m->create_input_connection(p_eat, 1, arena->get_distance_to_food_p());
  m->create_input_connection(p_stop_x, 1, arena->get_distance_to_food_x_p());
  m->create_input_connection(p_stop_y, 1, arena->get_distance_to_food_y_p());
  m->create_input_connection(p_speed_x, 1, arena->get_distance_to_food_x_p());
  m->create_input_connection(p_speed_y, 1, arena->get_distance_to_food_y_p());

  m->init();

  return m;
}

Machine *Machine_factory::create_jumping_ball( void )
{
  Machine *m = new Machine();

  // Create arena
  Arena_jumping_ball *arena = new Arena_jumping_ball();
  m->set_arena(arena);

  cout << " Creating machine: Jumping ball" << endl;

  // add predictors
  Predictor_target *p_cart_immo = new Predictor_target("Cart not moving");
  Predictor_const_v *p_speed_x = new Predictor_const_v("Speed X");
  Predictor_const_v *p_speed_0 = new Predictor_const_v("Speed 0");
  Predictor_const_v *p_speed_xm = new Predictor_const_v("Speed X m");
  Predictor_const_a *p_speed_y = new Predictor_const_a("Speed Y");
  Predictor_const_a *p_cart_speed = new Predictor_const_a("Cart acc");
  Predictor_const_a *p_cart_speed_m = new Predictor_const_a("Cart acc m");

  // parameters
  p_cart_immo->set_parameter(0.0);
  p_speed_x->set_parameter(1.5); // target velocity
  p_speed_0->set_parameter(0.0); // target velocity
  p_speed_xm->set_parameter(-1.5); // target velocity
  p_speed_y->set_parameter(-9.8); // gravity
  p_cart_speed->set_parameter(3.0); // gravity
  p_cart_speed_m->set_parameter(-3.0); // gravity

  Model *model_cart_immo = new Model(p_cart_immo);
  Model *model_speed_x = new Model(p_speed_x);
  Model *model_speed_0 = new Model(p_speed_0);
  Model *model_speed_xm = new Model(p_speed_xm);
  Model *model_speed_y = new Model(p_speed_y);
  Model *model_cart_speed = new Model(p_cart_speed);
  Model *model_cart_speed_m = new Model(p_cart_speed_m);

  m->add_model(model_cart_immo);
  m->add_model(model_speed_x);
  m->add_model(model_speed_0);
  m->add_model(model_speed_xm);
  m->add_model(model_speed_y);
  m->add_model(model_cart_speed);
  m->add_model(model_cart_speed_m);

  m->create_input_connection(p_cart_immo, 1, arena->get_position_p());
  m->create_input_connection(p_speed_x, 1, arena->get_ball_x_position_p());
  m->create_input_connection(p_speed_0, 1, arena->get_ball_x_position_p());
  m->create_input_connection(p_speed_xm, 1, arena->get_ball_x_position_p());
  m->create_input_connection(p_speed_y, 1, arena->get_ball_y_velocity_p());
  m->create_input_connection(p_speed_y, 2, arena->get_ball_y_position_p());
  m->create_input_connection(p_cart_speed, 1, arena->get_velocity_p());
  m->create_input_connection(p_cart_speed, 2, arena->get_position_p());
  m->create_input_connection(p_cart_speed_m, 1, arena->get_velocity_p());
  m->create_input_connection(p_cart_speed_m, 2, arena->get_position_p());

  // get force from output module
  Output_cart_ball *output = new Output_cart_ball;
  output->set_arena(arena);
  m->create_output_connection(model_cart_speed, arena->function_set_force(), "force on cart", output);
  m->create_output_connection(model_cart_speed_m, arena->function_set_force(), "force on cart", output);
  m->create_output_connection(model_cart_immo, arena->function_set_force(), "force on cart", output);

  return m;
}

Machine *Machine_factory::create_pole_cart( void )
{
  Machine *m = new Machine();

  // Create arena
  Arena_pole_cart *arena = new Arena_pole_cart();
  m->set_arena(arena);

  cout << " Creating machine: Pole cart" << endl;

  // add predictors
  Predictor_const_a *p_accelerate = new Predictor_const_a("Accelerate");
  Predictor_target *p_target = new Predictor_target("Target");
  //Predictor_target *p_force = new Predictor_force("Force");

  Model *model_acceleration = new Model(p_accelerate);
  Model *model_target = new Model(p_target);

  m->add_model(model_target);
  m->add_model(model_acceleration);

  //m->create_input_connection(p_target, 0, arena->get_position_p());

  return m;
}
